#!/usr/bin/python
# Filename: robo3pi.py

import serial

import time
import math
global ser


def init(com):
	global ser
	ser = serial.Serial(com, 57600, timeout=3)

def move(id, left, right):
	second ='5'
	if left<0:
		second = chr( ord(second) | 0x80)
	if right<0:
		second = chr( ord(second) | 0x40)
	_left = int(math.fabs(left))
	_right = int(math.fabs(right))
	__sendCommand(id ,second ,chr(_left),chr(_right))
	
def stop(id):
	__sendCommand(id ,'5' ,chr(0),chr(0))
	
def __sendCommand(id, first, second,thired):
	global ser
	ser.write(id)
	ser.write(first)
	ser.write(second)
	ser.write(thired)
	
def addToGroup(id, group):
	__sendCommand(id , '1', group ,chr(0))

def readRawSensors(id):
	__sendCommand(id , '6', chr(1), chr(0))
	return ser.readline()
	
def readCalibratedSensors(id,cal):
	__sendCommand(id , '6', chr(0), chr(cal))
	return ser.readline()
	

	return ser.readline()
def readLinePosition(id):
	__sendCommand(id , '8', chr(0), chr(0))
	return ser.readline()

def togglePID(id,speed,p_num,p_den,d_num,d_den):
	command = chr( ( int(speed&0xff) << 6) + ord('7'))
	num1 = (p_num<< 4) +p_den
	num2 = (d_num<< 4) +d_den
	__sendCommand(id , (command), (num1), (num2))
	
def operatePID(id):
	__sendCommand(id , '9', chr(0), chr(0))
	
def removeFromGroup(id, group):
	__sendCommand(id , '2', group ,chr(0))
	
def _battary(id):
	__sendCommand(id, '3', chr(0),chr(0))

def _identify(id):
	__sendCommand(id, '4', chr(0),chr(0))
	
def close():
	global ser
	try:
		ser.close()
	except:
		pass
		
# End of 3pi.py